/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Takes Occu map, a measurement set that is already matched and matches the measuements into the map
* 
*/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <string>
#include <vector>
#include "owndebug.h"
#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "OccuGUI.h"
#include "BetBdtLocalizer.h"
#include "ownutils.h"
#include "CParticleFilter.h"

#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

//(21.750000 , 24.379999 , -3.449088) SET1
// ./match2map rocla-map.occu rocla_match_1.txt 21.75 24.37 -3.45 map_matched_set1.txt

// SET 2 = (12.546875 , -16.593750 , 2.067495)
// ./match2map Global-Map.occu rocla_match_2.txt 12.54 -16.59 2.067 map_matched_set2.txt
///Initial pose
Grid::pose global_ref_pose;
std::string logfilename="scan_match_out.txt";

void timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
	
	if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
	else{
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
	}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// Read Scan measurement file with arbitrary number of items (and allowing white space before '\n')
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}

void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/// SLAM-MAP Matcher
/// Matches local SLAM maps to the global map
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void globalSLAMMatch(const char *meas_file, 
										CBetBdtLocalizer *glocalizer)
{
		CMatchStorage matches(50);
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		
		
		int enableVisualization = 0;
		FILE *logfilu = fopen(logfilename.c_str(),"wt");
		FILE *fdebug = fopen("debug_scans.txt","wt");
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		float max_laser_dist=45,maxLaserDistance=max_laser_dist;
		float enough_scan_poinsts=600;
		float edge_th=35.0;
		float grid_size = 75;
		
		
		Grid::TScan meas, first;
		meas.alloc(enough_scan_poinsts);
		first.alloc(enough_scan_poinsts);
		std::vector< std::vector<float> > scans; ///All scans for previous map
		std::vector<float> tmp_scan;
		
		Grid::pose init_est,diff;
		Grid::pose last,current;
		Grid::pose reference_pose, pose_now, global_search;
		
		///Global initial estimate and search area
		//global_ref_pose.set(20,20,0);
		global_search.set(2.0,2.0,M_PI/8);
		
		CEnvironmentMeasurement2D global_meas;
		global_meas.frame.setError(global_search.x,global_search.y,global_search.a);		
		
		
		
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		float greso=glocalizer->grid->getResolution();
		
		TOccupancyGrid O(grid_size,grid_size,greso,Grid::TCoord2D(0,0));
		O.setGridToValue(0.5);
		int oNx,oNy;
		O.getDim(oNx,oNy);
		unsigned char *grgb = (unsigned char *) malloc(Nx*Ny*sizeof(unsigned char));
		
		///Set the first scan
		readScanLine(log,first,current);
		//meas.filt(max_laser_dist);
		
		tmp_scan.clear();
		
		for(int i=0;i<meas.N;i++){
				tmp_scan.push_back(first.r[i]);
		}
		
		scans.push_back(tmp_scan);
		poses.clear();
		poses.push_back(init_est);
		last = current;
		int counter=0;
		
		///READ ALL THE MEASUREMENTS!
		while(!feof(log)){
				readScanLine(log,meas,current);
			
				//meas.filt(max_laser_dist);
				///Save scans and poses 
				tmp_scan.clear();
				
				for(int i=0;i<meas.N;i++){
					if(meas.r[i]>max_laser_dist) meas.r[i]=0;
						tmp_scan.push_back(meas.r[i]);
				}
				scans.push_back(tmp_scan);
				//fprintf(stderr,"%d %d\n",meas.N, tmp_scan.size());
				diff.setToDifferentialPose(current,last);
				init_est = init_est.integrateDifferential(diff);
				poses.push_back(init_est);
				last=current;
				
		}
		
		bool isFirst=true;
		int firstIndex=0;
		init_est.set(0,0,0);
		
		for(int measnum=0;measnum<poses.size();measnum++){
			///Copy scan data
			for(int j=0;j<meas.N;j++){
				meas.r[j]=scans[measnum][j];
				meas.a[j]=first.a[j];
				//meas.N = scans[measnum].size();
			}
			//fprintf(stderr,"XX %d %d\n",meas.N,scans[measnum].size());
			current = poses[measnum];

			if(isFirst){
					isFirst = false;
					O.setScanFillProb(0, 0 ,0, meas,0.3,0.95); ///This should be the last scan
					last=current;
					firstIndex = measnum;
				}else{
					diff.setToDifferentialPose(current,last);
					init_est = init_est.integrateDifferential(diff);
					O.setScanFillProb(init_est.x,init_est.y,init_est.a, meas,0.3,0.95); ///Fill the measurements into grid
					last=current;
					writeScanLine(fdebug,meas,init_est.x,init_est.y,init_est.a);
					fflush(fdebug);
				}
				//fprintf(stderr,"(%f, %f, %f)\n", init_est.x,init_est.y,init_est.a);
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				pos = init_est;
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				if(toEdge < edge_th){
						global_meas.setOccupancyGrid(&O, 0.8); ///< set grid to 2D measurement 
						global_meas.frame.setFrame(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);	
						glocalizer->localize(global_meas,matches);
						matches.sort();
						
						fprintf(stderr,"\n###################################################\n");
						fprintf(stderr,"%d Pose estimate = (%f , %f , %f)\n",measnum, matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						fprintf(stderr,"###################################################\n\n");
						 ///< this is the matched reference
						global_ref_pose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						
						
						
						///the first scan is missing
						for(int j=0;j<meas.N;j++){
							meas.r[j]=scans[firstIndex][j];
						}
						glocalizer->setMeasurementToMap(meas,global_ref_pose.x,global_ref_pose.y,global_ref_pose.a ,0.8,false);					
						///Add measurements to the map and save
						for(unsigned int i=firstIndex;i<measnum;i++){
								for(int j=0;j<meas.N;j++){
										meas.r[j]=scans[i+1][j];
								}
								diff.setToDifferentialPose(poses[i+1],poses[i]);
								global_ref_pose = global_ref_pose.integrateDifferential(diff);
								glocalizer->setMeasurementToMap(meas,global_ref_pose.x,global_ref_pose.y,global_ref_pose.a ,0.8,false);	
								writeScanLine(logfilu,meas, global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);
						}
					
						global_ref_pose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						reference_pose = global_ref_pose;
						global_ref_pose = global_ref_pose.integrateDifferential(init_est); ///< Current pose
						reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
						firstIndex = measnum;
						isFirst = true;
						pos.set(0,0,0); ///< initialize pose
						init_est.set(0,0,0);
						O.setGridToValue(0.5);
						glocalizer->updateDistTransform();
						
						glocalizer->grid->fillRGB(grgb, 1.0/255.0);
						char buf[50];
						sprintf(buf,"%05d_global-map.jpg",counter);
						saveJPEG(buf, grgb,Nx, Ny);
				}
				counter++;
				
				////////////////////////////////////////////////////////////////////////////////
				
		}///While()
		fclose(fdebug);
		fclose(logfilu);
}




/// ./match2map OccuMap MeasurementFile x y a logfilename
int main(int argc, char *argv[]){
		const char *measurement_file_name="../../../data/j2b2_measurement.txt";
		std::string mapfilename;
		int enough_scan_poinsts = 400; ///< enough to reserve memory for the scan points in the meas file.
		
		if(argc!=7){
			fprintf(stderr,"Usage: ./match2map OccuName Measfile X Y A output_logfile \n");
			fprintf(stderr,"X,Y,A is the initial pose\n");
			exit(1);
		}
		mapfilename = argv[1];
		measurement_file_name = argv[2];
		global_ref_pose.x = atof(argv[3]);
		global_ref_pose.y = atof(argv[4]);
		global_ref_pose.a = atof(argv[5]);
		logfilename = argv[6];
		
		
		///Localizers
		CBetBdtLocalizer *glocalizer; 
		glocalizer = new CBetBdtLocalizer(mapfilename.c_str());
		
		// Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(1);
		
		//globalSLAMMatch(settings,measurement_file_name,glocalizer,slammer,grgb,srgb);
		globalSLAMMatch(measurement_file_name,glocalizer);
			
		return 0;
}